
#include <stdlib.h>
#include <stdbool.h>
#include <errno.h>
#include <time.h>
#include <stdio.h>
#include <string.h>
#include <pthread.h>
#include <arpa/inet.h>
#include "config.h"
#include "zlog.h"

#ifndef ASSERT_RET
#define __ASSERT__(err, fmt, ...)   \
do {                    \
    if ((err)) {        \
        zlog_error(gZlog, "%s(), line %d, error: %s "fmt, __FUNCTION__, __LINE__,  \
                #err, ##__VA_ARGS__);    \
        return -EINVAL; \
    }                   \
} while ((void)0, 0)

#define ASSERT_RET(err)             __ASSERT__(err, "")
#define ASSERT_INF(err, fmt, ...)   __ASSERT__(err, ": "fmt, ##__VA_ARGS__)
#endif

#define PRO_RLOCK(c)        (-pthread_mutex_lock  (&(chan)->rlock)                     )
#define PRO_WLOCK(c)        (-pthread_mutex_lock  (&(chan)->wlock)                     )
#define PRO_FLOCK(c)        (-pthread_mutex_lock  (&(chan)->flock)                     )
#define PRO_RUNLOCK(c)      (-pthread_mutex_unlock(&(chan)->rlock)                     )
#define PRO_WUNLOCK(c)      (-pthread_mutex_unlock(&(chan)->wlock)                     )
#define PRO_FUNLOCK(c)      (-pthread_mutex_unlock(&(chan)->flock)                     )
#define PRO_TWAIT(c,t)      (-pthread_cond_timedwait(&(chan)->fcond, &(chan)->flock, t))
#define PRO_TRIG(c)         (-pthread_cond_broadcast(&(chan)->fcond)                   )

#include "libprotocol.h"

#define OFFSET(T, M)        ((unsigned long)(&(((T *)(0))->M)))
#define round_up(x, y)      ((((x) + (y) - 1) / (y)) * (y))

static inline bool is_waiting_sub_ack(ProChan *chan)
{
    return chan->fop1 >= 0 &&
           chan->fop2 >= 0 &&
           chan->fseq >= 0 &&
           chan->ferr == SUCCEED;
}

static inline bool is_sub_ack_matched(ProChan *chan, ProHead *phead)
{
    return chan->fop1 == phead->opcode1 &&
           chan->fop2 == phead->opcode2;
}

static inline bool is_last_frame(ProHead *phead)
{
    return phead->frame_cur >= phead->frame_sum;
}

static inline bool is_multi_frame(ProHead *phead)
{
    return phead->frame_sum > 1;
}

static int pro_wait_signal(ProChan *chan)
{
    int ret = -1;
    struct timespec ts = {0};

    ret = PRO_FLOCK(chan);
    if (ret) {
        zlog_error(gZlog, "Failed to lock! chanid[%d]", chan->chanid);
        return ret;
    }

    ts.tv_sec = time(NULL) + 4;
    while (false == chan->fack) {
        zlog_debug(gZlog, "Before wait trig chanid[%d]", chan->chanid);
        ret = PRO_TWAIT(chan, &ts);
        if (!ret)
            continue;

        if (ret == -ETIMEDOUT) {
            zlog_error(gZlog, "Wait condition timeout! chanid[%d]", chan->chanid);
            break;
        }

        zlog_error(gZlog, "Failed to wait condition! chanid[%d], ret[%d]", chan->chanid, ret);
        break;
    }

    chan->fack = false;

    if ( PRO_FUNLOCK(chan) ) {
        zlog_error(gZlog, "Failed to unlock! chanid[%d]", chan->chanid);
        return -EFAULT;
    }

    return ret;
}

static inline int pro_read(ProChan *chan, void *buff, int len)
{
    ASSERT_INF(!chan->read, "No reading callback function, chanid[%d]!", chan->chanid);
    return chan->read(chan->id, buff, len);
}

static inline int pro_write(ProChan *chan, void *buff, int len)
{
    ASSERT_INF(!chan->read, "No writing callback function, chanid[%d]!", chan->chanid);
    return chan->write(chan->id, buff, len);
}

static inline FrameList *frame_find_last(FrameList *head)
{
    while (head->next)
        head = head->next;

    return head;
}

static FrameList *frame_alloc(int len)
{
    FrameList *frame = NULL;
    ProHead *head  = NULL;

    frame = calloc(1, len + sizeof(*frame));
    if (!frame) {
        zlog_error(gZlog, "Failed to alloc protocol frame, size[%zd]", len + sizeof(*frame));
        return NULL;
    }

    frame->next = NULL;
    frame->len  = len;
    frame->data = (char *)frame + OFFSET(FrameList, data) + sizeof(frame->data);

    head = (ProHead *)frame->data;
    head->magic = htonl(PRO_MAGIC_HEAD);
    head->version = htonl(PRO_VERSION_2);
    head->frame_len = htonl(len);
    return frame;
}

static void frame_free(FrameList *list)
{
    FrameList *next = NULL;

    if (!list)
        return ;

    while (list) {
        next = list->next;
        free(list);
        list = next;
    }
}

static void frame_read_cleanup(void *arg)
{
    int ret = 0;
    ProChan *chan = (ProChan *)arg;

    zlog_debug(gZlog, "Protocol layer cleaning, chanid[%d]", chan->chanid);
    ret = PRO_RUNLOCK(chan);
    if (ret)
        zlog_error(gZlog, "Failed to put protocol read lock, chanid[%d], ret[%d]",
                chan->chanid, ret);
}

static int frame_read_length(ProChan *chan)
{
    /* if we want to check more header tags, just add other elements here */
    unsigned int magic = 0;     // should be equals to proto_head->magic
    unsigned int version = 0;   // should be equals to proto_head->version
    unsigned int frame_len = 0;
    int ret = -1;

    /* locate protocol header magic number */
    for (;;) {
        magic   = (magic << 8)   | (version >> 24);		// shift the highest byte into magic
        version = (version << 8) | (frame_len >> 24);   // shift one byte
        frame_len = (frame_len << 8);                   // shift one byte

        /* read one byte to the lowest byte in 'frame_len' */
        /* this operate assumes local machine is little endian (certainly) */
        pthread_cleanup_push(frame_read_cleanup, chan);
        ret = pro_read(chan, &frame_len, 1);
        pthread_cleanup_pop(0);
        if (ret < 0)
            return ret;

        /* to make the frame receving thread cancelable */
        /* return -EAGAIN to exit protocol layer */
        /* the receving thread should ignore the -EAGAIN error and add cancel point */
        if (ret == 0)
            return -EAGAIN;

        zlog_debug(gZlog, "[0x%08x %08x %08x] chanid[%d]", magic, version, frame_len, chan->chanid);
        /* head magic and version is turned to little endian by byte shifting */
        if ( (magic   != PRO_MAGIC_HEAD) ||
             (version != PRO_VERSION_2 ) ||
             (frame_len < PRO_MIN_SIZE ) ) {
            continue;
        }

        /* got an valid frame now, breakout */
        zlog_debug(gZlog, "New frame incoming, frame length[%d], chanid[%d]",
                frame_len, chan->chanid);
        break;
    }

    /* return the frame length to upper functions */
    return frame_len;
}

/* used by crc32 calculate */
static const unsigned int table[256] = {
0x00000000,0x77073096,0xee0e612c,0x990951ba,0x076dc419,0x706af48f,0xe963a535,0x9e6495a3,
0x0edb8832,0x79dcb8a4,0xe0d5e91e,0x97d2d988,0x09b64c2b,0x7eb17cbd,0xe7b82d07,0x90bf1d91,
0x1db71064,0x6ab020f2,0xf3b97148,0x84be41de,0x1adad47d,0x6ddde4eb,0xf4d4b551,0x83d385c7,
0x136c9856,0x646ba8c0,0xfd62f97a,0x8a65c9ec,0x14015c4f,0x63066cd9,0xfa0f3d63,0x8d080df5,
0x3b6e20c8,0x4c69105e,0xd56041e4,0xa2677172,0x3c03e4d1,0x4b04d447,0xd20d85fd,0xa50ab56b,
0x35b5a8fa,0x42b2986c,0xdbbbc9d6,0xacbcf940,0x32d86ce3,0x45df5c75,0xdcd60dcf,0xabd13d59,
0x26d930ac,0x51de003a,0xc8d75180,0xbfd06116,0x21b4f4b5,0x56b3c423,0xcfba9599,0xb8bda50f,
0x2802b89e,0x5f058808,0xc60cd9b2,0xb10be924,0x2f6f7c87,0x58684c11,0xc1611dab,0xb6662d3d,
0x76dc4190,0x01db7106,0x98d220bc,0xefd5102a,0x71b18589,0x06b6b51f,0x9fbfe4a5,0xe8b8d433,
0x7807c9a2,0x0f00f934,0x9609a88e,0xe10e9818,0x7f6a0dbb,0x086d3d2d,0x91646c97,0xe6635c01,
0x6b6b51f4,0x1c6c6162,0x856530d8,0xf262004e,0x6c0695ed,0x1b01a57b,0x8208f4c1,0xf50fc457,
0x65b0d9c6,0x12b7e950,0x8bbeb8ea,0xfcb9887c,0x62dd1ddf,0x15da2d49,0x8cd37cf3,0xfbd44c65,
0x4db26158,0x3ab551ce,0xa3bc0074,0xd4bb30e2,0x4adfa541,0x3dd895d7,0xa4d1c46d,0xd3d6f4fb,
0x4369e96a,0x346ed9fc,0xad678846,0xda60b8d0,0x44042d73,0x33031de5,0xaa0a4c5f,0xdd0d7cc9,
0x5005713c,0x270241aa,0xbe0b1010,0xc90c2086,0x5768b525,0x206f85b3,0xb966d409,0xce61e49f,
0x5edef90e,0x29d9c998,0xb0d09822,0xc7d7a8b4,0x59b33d17,0x2eb40d81,0xb7bd5c3b,0xc0ba6cad,
0xedb88320,0x9abfb3b6,0x03b6e20c,0x74b1d29a,0xead54739,0x9dd277af,0x04db2615,0x73dc1683,
0xe3630b12,0x94643b84,0x0d6d6a3e,0x7a6a5aa8,0xe40ecf0b,0x9309ff9d,0x0a00ae27,0x7d079eb1,
0xf00f9344,0x8708a3d2,0x1e01f268,0x6906c2fe,0xf762575d,0x806567cb,0x196c3671,0x6e6b06e7,
0xfed41b76,0x89d32be0,0x10da7a5a,0x67dd4acc,0xf9b9df6f,0x8ebeeff9,0x17b7be43,0x60b08ed5,
0xd6d6a3e8,0xa1d1937e,0x38d8c2c4,0x4fdff252,0xd1bb67f1,0xa6bc5767,0x3fb506dd,0x48b2364b,
0xd80d2bda,0xaf0a1b4c,0x36034af6,0x41047a60,0xdf60efc3,0xa867df55,0x316e8eef,0x4669be79,
0xcb61b38c,0xbc66831a,0x256fd2a0,0x5268e236,0xcc0c7795,0xbb0b4703,0x220216b9,0x5505262f,
0xc5ba3bbe,0xb2bd0b28,0x2bb45a92,0x5cb36a04,0xc2d7ffa7,0xb5d0cf31,0x2cd99e8b,0x5bdeae1d,
0x9b64c2b0,0xec63f226,0x756aa39c,0x026d930a,0x9c0906a9,0xeb0e363f,0x72076785,0x05005713,
0x95bf4a82,0xe2b87a14,0x7bb12bae,0x0cb61b38,0x92d28e9b,0xe5d5be0d,0x7cdcefb7,0x0bdbdf21,
0x86d3d2d4,0xf1d4e242,0x68ddb3f8,0x1fda836e,0x81be16cd,0xf6b9265b,0x6fb077e1,0x18b74777,
0x88085ae6,0xff0f6a70,0x66063bca,0x11010b5c,0x8f659eff,0xf862ae69,0x616bffd3,0x166ccf45,
0xa00ae278,0xd70dd2ee,0x4e048354,0x3903b3c2,0xa7672661,0xd06016f7,0x4969474d,0x3e6e77db,
0xaed16a4a,0xd9d65adc,0x40df0b66,0x37d83bf0,0xa9bcae53,0xdebb9ec5,0x47b2cf7f,0x30b5ffe9,
0xbdbdf21c,0xcabac28a,0x53b39330,0x24b4a3a6,0xbad03605,0xcdd70693,0x54de5729,0x23d967bf,
0xb3667a2e,0xc4614ab8,0x5d681b02,0x2a6f2b94,0xb40bbe37,0xc30c8ea1,0x5a05df1b,0x2d02ef8d,
};

/* TODO optimize frame_crc32 input parameters */
static unsigned int frame_crc32(ProHead *head, char *data, int datasize, int padsize)
{
    unsigned int crc = 0xFFFFFFFF;
    unsigned char sn = 0;
    char *pbuf = NULL;
    int len = 0;

    /* protocol head, start from proto_head->version */
    pbuf = (char *)&head->version;
    len  = sizeof(*head) - OFFSET(ProHead, version);
    while (len--) {
        sn = (unsigned char)crc ^ *pbuf++;
        crc = table[sn] ^ (crc >> 8);
    }

    /* payload */
    pbuf = data;
    len  = datasize; 
    while (len--) {
        sn = (unsigned char)crc ^ *pbuf++;
        crc = table[sn] ^ (crc >> 8);
    }

    /* padding zeros */
    while (padsize--) {
        sn = (unsigned char)crc ^ 0;
        crc = table[sn] ^ (crc >> 8);
    }

    return crc ^= 0xFFFFFFFF;
}

static int frame_check(ProChan *chan, FrameList *frame)
{
    unsigned int crc32 = 0;
    ProHead *head = PRO_HEAD(frame);
    ProTail *tail = PRO_TAIL(frame);
    char *payload = PRO_PAYLOAD(frame);
    int   paylen  = PRO_PAYLEN(frame);

    tail->magic = ntohl(tail->magic);
    tail->crc32 = ntohl(tail->crc32);

    if ( PRO_MAGIC_TAIL != tail->magic ) {
        zlog_info(gZlog, "Invalid protocol tail magic, expect[0x%8x], got[0x%08x], chanid[%d]",
                PRO_MAGIC_TAIL, tail->magic, chan->chanid);
        goto err;
    }

    if (head->flags & PRO_FLAG_CRC)
        crc32 = frame_crc32(head, payload, paylen, head->padsize);

    if ( 0 != crc32 && crc32 != tail->crc32 ) {
        zlog_info(gZlog, "CRC32 check failed! expect[0x%08X], got[0x%08X], chanid[%d]", 
                tail->crc32, crc32, chan->chanid);
        goto err;
    }

    head->magic     = ntohl(head->magic);
    head->version   = ntohl(head->version);
    head->frame_len = ntohl(head->frame_len);
    head->flags     = ntohl(head->flags);
    head->frame_cur = ntohs(head->frame_cur);
    head->frame_sum = ntohs(head->frame_sum);
    head->payload_len = ntohl(head->payload_len);
    head->opcode2 = ntohs(head->opcode2);

    if ( !head->frame_cur || !head->frame_sum || 
          head->frame_cur >   head->frame_sum ) {
        zlog_info(gZlog, "Invalid frame number, cur[%d], sum[%d], chanid[%d]!", 
                head->frame_cur, head->frame_sum, chan->chanid);
        goto err;
    }

    if (chan->prod_type && chan->prod_type != head->product_type) {
        zlog_info(gZlog, "Invalid product type, expect[%d], got[%d], chanid[%d]!", 
                chan->prod_type, head->product_type, chan->chanid);
        goto err;
    }

    if (chan->prod_ver && chan->prod_ver != head->product_ver) {
        zlog_info(gZlog, "Invalid product version, expect[%d], got[%d], chanid[%d]!", 
                chan->prod_ver, head->product_ver, chan->chanid);
        goto err;
    }

    if (head->padsize && ((paylen + head->padsize) % PRO_DATA_ALIGN)) {
        zlog_info(gZlog, "Sum of payload[%d] and padsize[%d] not aligned to [%d], chanid[%d]!", 
                paylen, head->padsize, PRO_DATA_ALIGN, chan->chanid);
        goto err;
    }

    if (is_waiting_sub_ack(chan) && is_sub_ack_matched(chan, head) &&
        head->frame_sum != 1) {
        /* To compatible with MCU bootload upgrade, it's fr_sum in ACK is same to REQ */
        zlog_debug(gZlog, "Correcting SUB-ACK frame_sum[%d] to [1]", head->frame_sum);
        head->frame_sum = 1;
    }

    zlog_debug(gZlog, "Frame check succeed, chanid[%d]!", chan->chanid);
    return 0;

err:
    zlog_error(gZlog, "Frame check failed, chanid[%d]!", chan->chanid);
    return -EINVAL;
}

static void frame_append(ProChan *chan, FrameList *frame)
{
    FrameList *last = NULL;
    ProHead *old = NULL;
    ProHead *new = PRO_HEAD(frame);

    zlog_debug(gZlog, "Recv: chanid[%d] cnt[%ld] op1[0x%x] op2[%d] err[%d] "
           "cur[%d] sum[%d] seq[%d] paysize[%d] framelen[%d]",
            chan->chanid, chan->recv_cnt, new->opcode1, new->opcode2, new->errcode,
            new->frame_cur, new->frame_sum, new->frame_seq,
            PRO_PAYLEN(frame), new->frame_len);

    /* No old frame list */
    if (!chan->list_head)
        goto out;

    /* got old frame list, we are receiving sub-frame */
    last = frame_find_last(chan->list_head);
    old = PRO_HEAD(last);

    if (old->opcode1 != new->opcode1 || old->opcode2 != new->opcode2) {
        /* opcode mis-match, new session, drop the old frame list and use the new one */
        zlog_error(gZlog, "Old op1[0x%x] op2[0x%x], new op1[0x%x] op2[0x%x], chanid[%d], drop old",
                old->opcode1, old->opcode2, new->opcode1, new->opcode2, chan->chanid);
        frame_free(chan->list_head);
        goto out;
    }

    /* opcode matched, the new frame is a sub-frame */

    if (old->frame_cur != new->frame_cur - 1) {
        zlog_error(gZlog, "frame_cur mis-match, old[%d], new[%d], chanid[%d], drop new frame",
                old->frame_cur, new->frame_cur, chan->chanid);
        frame_free(frame);
        return ;
    }

    if (old->frame_sum != new->frame_sum) {
        zlog_error(gZlog, "frame_sum mis-match, old[%d], new[%d], chanid[%d], drop new frame",
                old->frame_sum, new->frame_sum, chan->chanid);
        frame_free(frame);
        return ;
    }

    if (old->frame_seq != ((new->frame_seq - 1) & 0xFF)) {
        zlog_error(gZlog, "frame_seq mis-match, old[%d], new[%d], chanid[%d], drop new frame",
                old->frame_seq, new->frame_seq, chan->chanid);
        frame_free(frame);
        return;
    } 

    /* valid sub-frame, append it to the old sub-frame list and return */
    last->next = frame;
    return;

out:
    /* we got a new frame list */
    chan->list_head = frame;
}

static int frame_read_rest(ProChan *chan, FrameList *frame)
{
    int ret = -1;
    ProHead *head = PRO_HEAD(frame);
    int len = frame->len - OFFSET(ProHead, flags);
    
    /* previous bytes are readed out by frame_read_length() */
    ret = pro_read(chan, &head->flags, len);
    if (ret != len) {
        zlog_error(gZlog, "Failed to read total protocol frame, len[%d], ret[%d], chanid[%d]",
                len, ret, chan->chanid);
        return -EFAULT;
    }

    chan->recv_cnt++;
    zlog_debug(gZlog, "Total frame are readed out, len[%d], recv_cnt[%ld], chanid[%d]",
            len, chan->recv_cnt, chan->chanid);
    /* PR_MEM(frame->data, frame->len); */
    return 0;
}

static int pro_recv_sub_frame(ProChan *chan, FrameList **newframe)
{
    int ret = 0;
    FrameList *frame = NULL;
	//SenLink *link = (SenLink *)chan->

    /* find protocol head and read frame length */
    ret = frame_read_length(chan);
    if (ret < 0) {
        if (ret != -EAGAIN)
            zlog_error(gZlog, "Failed to read frame length, chanid[%d]", chan->chanid);
        return ret;
    }

    /* alloc a new frame */
    frame = frame_alloc(ret);
    if (!frame) {
        zlog_error(gZlog, "Failed to alloc memory for new frame, length[%d], chanid[%d]",
                ret, chan->chanid);
        return -ENOMEM;
    }

    zlog_debug(gZlog, "New frame allocated, frame length[%d], chanid[%d]", ret, chan->chanid);

    /* read out the rest bytes of this protocol frame */
    ret = frame_read_rest(chan, frame);
    if (ret < 0) {
        zlog_error(gZlog, "Failed to read rest bytes of this protocol frame, chanid[%d]",
                chan->chanid);
        goto err;
    }

    /* check frame */
    ret = frame_check(chan, frame);
    if (ret < 0)
        goto err;

    *newframe = frame;
    return 0;

err:
    frame_free(frame);
    return ret;
}

static int pro_send_sub_ack(ProChan *chan, ProHead *phead);
int proFramesRecv(ProChan *chan, FrameList **list)
{
    int ret = -1;
    ProHead *phead = NULL;
	FrameList *new = NULL;

    ASSERT_RET(!chan);
    ASSERT_INF(!chan->read, "Init this protocol channel before destroy!");
    ASSERT_RET(!list);

    if (chan->chanid < 0) {
        zlog_error(gZlog, "Initialize ISVISION protocol channel first!");
        return -EFAULT;
    }

    zlog_debug(gZlog, "Receiving frame from chanid[%d]...", chan->chanid);

    ret = PRO_RLOCK(chan);
    if (ret) {
        zlog_error(gZlog, "Failed to get proto read lock, chanid[%d], ret[%d]", chan->chanid, ret);
        return ret;
    }

    do {
        ret = pro_recv_sub_frame(chan, &new);
        if (ret < 0) {
            if (ret != -EAGAIN)
                zlog_error(gZlog, "Failed to receive sub frame from chanid[%d]", chan->chanid);
            goto out;
        }

        frame_append(chan, new);

        phead = PRO_HEAD(new);

        if ( is_multi_frame(phead) ) {
            ret = pro_send_sub_ack(chan, phead);
            if (ret < 0) {
                zlog_error(gZlog, "Failed to send sub-frame ack, chanid[%d]", chan->chanid);
                goto out;
            }
        }
    } while (!is_last_frame(phead));

    /* all sub frames received, return the frame list */
    *list = chan->list_head;

out:
    if (ret < 0 && ret != -EAGAIN) {
        frame_free(chan->list_head);
        chan->list_head = NULL;
    }

    if (0 != PRO_RUNLOCK(chan)) {
        zlog_error(gZlog, "Failed to put protocol read lock, chanid[%d]", chan->chanid);
        return -EFAULT;
    }

    return ret;
}

static int pro_send_sub_frame(ProChan *chan, char *payload, int size,
                              int fr_cur, int fr_sum, int fr_seq, int paylen,
                              int op1, int op2, int err)
{
    int ret = -1;
    unsigned int crc32 = 0;
    int padsize = round_up(size, PRO_DATA_ALIGN) - size;
    ProHead *head = NULL;
    ProTail *tail = NULL;
    char zero[PRO_DATA_ALIGN] = {0};
    char *buff = NULL;
    int frame_len = sizeof(*head) + size + padsize + sizeof(*tail);
    int len = 0;

    buff = calloc(1, frame_len);
    if (!buff) {
        zlog_error(gZlog, "Failed to alloc frame memory, size[%d], chanid[%d]",
                frame_len, chan->chanid);
        return -ENOMEM;
    }

    head = (ProHead *)(buff + len);
    len += sizeof(*head);

    head->magic        = htonl(PRO_MAGIC_HEAD);
    head->version      = htonl(PRO_VERSION_2);
    head->frame_len    = htonl(frame_len);
    head->flags        = htonl(PRO_FLAG_CRC);
    head->frame_cur    = htons((unsigned short)fr_cur);
    head->frame_sum    = htons((unsigned short)fr_sum);
    head->payload_len  = htonl(paylen);
    head->product_type = (unsigned char)chan->prod_type;
    head->product_ver  = (unsigned char)chan->prod_ver;
    head->padsize      = (unsigned char)padsize;
    head->frame_seq    = (unsigned char)fr_seq;
    head->opcode2      = htons((unsigned short)op2);
    head->opcode1      = (unsigned char)op1;
    head->errcode      = (unsigned char)err;

    memcpy(buff + len, payload, size);
    len += size;

    memcpy(buff + len, zero, padsize);
    len += padsize;

    crc32 = frame_crc32(head, payload, size, padsize);
        
    tail = (ProTail *)(buff + len);
    len += sizeof(*tail);

    tail->crc32 = htonl(crc32);
    tail->magic = htonl(PRO_MAGIC_TAIL);

    ret = pro_write(chan, buff, len);
    if (ret < 0) {
        zlog_error(gZlog, "Failed to send frame, cur[%d] sum[%d] seq[%d], chanid[%d]",
                fr_cur, fr_sum, fr_seq, chan->chanid);
    } else {
        chan->send_cnt++;
        zlog_debug(gZlog, "Send: chanid[%d] cnt[%ld] op1[0x%x] op2[%d] err[%d] "
               "cur[%d] sum[%d] seq[%d] paysize[%d] framelen[%d]",
                chan->chanid, chan->send_cnt, op1, op2, err,
                fr_cur, fr_sum, fr_seq, size, frame_len);
        /* PR_MEM(buff, len); */
    }

    free(buff);
    return ret;
}

static int pro_send_sub_ack(ProChan *chan, ProHead *head)
{
    int ret = -1;
    int op1 = head->opcode1;
    int op2 = head->opcode2;
    int seq = head->frame_seq;

    ret = PRO_WLOCK(chan);
    if (ret) {
        zlog_error(gZlog, "Failed to get protocol write lock, chanid[%d], ret[%d]",
                chan->chanid, ret);
        return ret;
    }

    ret = pro_send_sub_frame(chan, NULL, 0,
                             1, 1, seq, 0,
                             op1, op2, 0);
    if (ret < 0) {
        zlog_error(gZlog, "Failed to send sub-ack, chanid[%d], op1[0x%x], op2[%d], seq[%d]",
                chan->chanid, op1, op2, seq);
    } else {
        zlog_debug(gZlog, "Succeed to send sub-ack, chanid[%d], op1[0x%x], op2[%d], seq[%d]",
                chan->chanid, op1, op2, seq);
    }

    if ( PRO_WUNLOCK(chan) ) {
        zlog_error(gZlog, "Failed to put protocol write lock, chanid[%d]", chan->chanid);
        return -EFAULT;
    }

    return ret;
}

#define PRO_MULTI_RETRY     3
static int pro_send_sub_wait_ack(ProChan *chan, char *payload, int size,
                                 int cur, int sum, int seq, int paylen,
                                 int op1, int op2, int err)
{
    int ret = -1;
    int retry = PRO_MULTI_RETRY;

    chan->fop1 = op1;
    chan->fop2 = op2;
    chan->fseq = (unsigned char)seq;

    while (retry--) {
        ret = pro_send_sub_frame(chan, payload, size,
                                 cur, sum, seq, paylen,
                                 op1, op2, err);
        if (ret < 0)
            break;

        ret = pro_wait_signal(chan);
        if (ret == -ETIMEDOUT) {
            zlog_error(gZlog, "Wait sub-frame ack timeout, cur[%d], sum[%d], seq[%d], chanid[%d]",
                    cur, sum, seq, chan->chanid);
            continue;
        }

        if (chan->ferr != SUCCEED) {
            zlog_error(gZlog, "Multi-frame ack error[%d], chanid[%d]", chan->ferr, chan->chanid);
            ret = -EFAULT;
        }
        break;
    }

    chan->fop1 = -1;
    chan->fop2 = -1;
    chan->fseq = -1;

    return ret;
}

int proFramesSend(ProChan *chan, char *payload, int size, int op1, int op2, int err)
{
    int ret = -1;
    int fr_cur = 0;
    int fr_sum = 0;
    int payload_len = round_up(size, PRO_DATA_ALIGN);
    int sendsize = 0;
    int fr_seq = 0;

    ASSERT_RET(!chan);
    ASSERT_RET(!!payload ^ !!size);
    ASSERT_INF(!chan->read, "Initialize ISVISION protocol channel first!");

    fr_sum = (size + chan->max_paylen - 1) / chan->max_paylen;
    ret = PRO_WLOCK(chan);
    if (ret) {
        zlog_error(gZlog, "Failed to get protocol write lock, chanid[%d], ret[%d]",
                chan->chanid, ret);
        return ret;
    }

    if (!payload || size <= chan->max_paylen) {
        /* if this frame is an ACK frame and opcode matched, use the recieved fr_seq */
        if (chan->list_head && PRO_HEAD(chan->list_head)->opcode1 == op1  &&
                               PRO_HEAD(chan->list_head)->opcode2 == op2) {
            fr_seq = PRO_HEAD(chan->list_head)->frame_seq;
        }

        ret = pro_send_sub_frame(chan, payload, size,
                                 1, 1, fr_seq, payload_len,
                                 op1, op2, err);
        goto out;
    }

    for (fr_cur = 1; size > chan->max_paylen; fr_cur++) {
        ret = pro_send_sub_wait_ack(chan, payload, chan->max_paylen,
                                    fr_cur, fr_sum, fr_cur, payload_len,
                                    op1, op2, err);
        if (ret < 0)
            goto out;

        payload += chan->max_paylen;
        size -= chan->max_paylen;
    }

    ret = pro_send_sub_wait_ack(chan, payload, size,
                                fr_cur, fr_sum, fr_cur, payload_len,
                                op1, op2, err);

out:
    if (ret < 0) {
        zlog_error(gZlog, "Failed to send frame, chanid[%d], payload[%p], size[%d]",
                chan->chanid, payload, sendsize);
        zlog_error(gZlog, "    fr_cur[%d], fr_sum[%d], paylen[%d], op1[0x%x], op2[%d], err[%d]",
                fr_cur, fr_sum, payload_len, op1, op2, err);
    }

    if ( PRO_WUNLOCK(chan) ) {
        zlog_error(gZlog, "Failed to put protocol write lock, chanid[%d]", chan->chanid);
        return -EFAULT;
    }

    return ret;
}

int proChanInit(ProChan *chan, int chanid,
                void *id, ProRdWrFunc read, ProRdWrFunc write, ProParseExt parse,
                int type, int ver)
{
    int ret = 0;

    ASSERT_RET(!chan);
    ASSERT_RET(chanid < 0);
    ASSERT_RET(!read);
    ASSERT_RET(!write);
    ASSERT_RET(!parse);
    ASSERT_RET(type < 0);
    ASSERT_RET(ver < 0);
    ASSERT_INF(chan->read, "De-init this protocol channel before initialization!");
    memset(chan, 0, sizeof(*chan));

    chan->chanid = chanid;
    ret = pthread_mutex_init(&chan->rlock, NULL);
    if (ret) {
        zlog_error(gZlog, "Failed to init protocol read lock! ret[%d], chanid[%d]",
                ret, chan->chanid);
        goto err_rlock;
    }

    ret = pthread_mutex_init(&chan->wlock, NULL);
    if (ret) {
        zlog_error(gZlog, "Failed to init protocol write lock! ret[%d], chanid[%d]",
                ret, chan->chanid);
        goto err_wlock;
    }

    ret = pthread_mutex_init(&chan->flock, NULL);
    if (ret) {
        zlog_error(gZlog, "Failed to init protocol multiframe lock! ret[%d], chanid[%d]",
                ret, chan->chanid);
        goto err_flock;
    }

    ret = pthread_cond_init(&chan->fcond, NULL);
    if (ret) {
        zlog_error(gZlog, "Failed to init protocol multiframe condition! ret[%d], chanid[%d]",
                ret, chan->chanid);
        goto err_fcond;
    }

    chan->id = id;
    chan->read = read;
    chan->write = write;
    chan->parse = parse;
    chan->prod_type = type;
    chan->prod_ver = ver;
    chan->chanid = chanid;
    chan->max_paylen = PAY_MAX_SIZE;
    chan->fack = false;
    chan->fop1 = -1;
    chan->fop2 = -1;
    chan->fseq = -1;
    chan->ferr = SUCCEED;
    chan->recv_cnt = 0;
    chan->send_cnt = 0;

    zlog_info(gZlog, "Protocol channel[%d] initialized for product[0x%X], ver[%d]!",
            chan->chanid, type, ver);
    return 0;

err_fcond:
    pthread_mutex_destroy(&chan->flock);
err_flock:
    pthread_mutex_destroy(&chan->wlock);
err_wlock:
    pthread_mutex_destroy(&chan->rlock);
err_rlock:
    zlog_error(gZlog, "Failed to initial protocol channel[%d]", chan->chanid);
    return ret;
}

int proChanDestroy(ProChan *chan)
{
    int ret = 0;

    ASSERT_RET(!chan);
    ASSERT_INF(!chan->read, "Init this protocol channel before destroy!");

    ret = pthread_mutex_destroy(&chan->rlock);
    if (ret) {
        zlog_error(gZlog, "Failed to de-init protocol read locks! ret[%d], chanid[%d]",
                ret, chan->chanid);
    }

    ret = pthread_mutex_destroy(&chan->wlock);
    if (ret) {
        zlog_error(gZlog, "Failed to de-init protocol write locks! ret[%d], chanid[%d]",
                ret, chan->chanid);
    }

    ret = pthread_mutex_destroy(&chan->flock);
    if (ret) {
        zlog_error(gZlog, "Failed to de-init protocol multiframe locks! ret[%d], chanid[%d]",
                ret, chan->chanid);
    }

    ret = pthread_cond_destroy(&chan->fcond);
    if (ret) {
        zlog_error(gZlog, "Failed to de-init protocol multiframe condition! ret[%d], chanid[%d]",
                ret, chan->chanid);
    }

    memset(chan, 0, sizeof(*chan));
    return ret;
}

static int pro_handle_sub_ack(ProChan *chan, ProHead *phead)
{
    int ret = -1;
    int op1 = -1;
    int op2 = -1;
    int err = -1;
    int seq = -1;

    /* not waiting sub-frame ack now, return error and continue parsing */
    if (!is_waiting_sub_ack(chan))
        return -EAGAIN;

    op2 = phead->opcode2;
    op1 = phead->opcode1;
    err = phead->errcode;
    seq = phead->frame_seq;

    /* waiting sub-frame ack, but opcode not matched, return error and continue parsing */
    if (!is_sub_ack_matched(chan, phead)) {
        return -EAGAIN;
    }

    /* opcode matched, but sequence number not, maybe double ack, drop it */
    if (chan->fseq != seq ) {
        zlog_debug(gZlog, "Sub-frame ACK sequence error, drop it, chanid[%d]", chan->chanid);
        zlog_debug(gZlog, "  exp op1[0x%x], op2[0x%0x], seq[%d]", chan->fop1, chan->fop2, chan->fseq);
        zlog_debug(gZlog, "  got op1[0x%x], op2[0x%0x], seq[%d]", op1, op2, seq);
        goto out;
    }

    ret = PRO_FLOCK(chan);
    if (ret) {
        zlog_error(gZlog, "Failed to lock! chanid[%d], ret[%d]", chan->chanid, ret);
        goto out;
    }

    chan->fack = true;
    chan->ferr = err;

    zlog_debug(gZlog, "Before trig chanid[%d], op1[0x%x], op2[0x%x], seq[%d]",
            chan->chanid, op1, op2, seq);
    ret = PRO_TRIG(chan);
    if (ret) {
        zlog_error(gZlog, "Failed to send signal! chanid[%d], ret[%d]", chan->chanid, ret);
        goto out;
    }

    ret = PRO_FUNLOCK(chan);
    if (ret) {
        zlog_error(gZlog, "Failed to unlock! chanid[%d], ret[%d]", chan->chanid, ret);
        goto out;
    }

out:
    /* always return 0, because ack frame is no need to parse */
    return 0;
}

static char *pro_payload_merge(FrameList *frames, int *paylen)
{
    ProHead *phead = PRO_HEAD(frames);
    char *paybuff = NULL;
    int offset = 0;

    paybuff = calloc(1, phead->payload_len);
    if (!paybuff) {
        zlog_error(gZlog, "Failed to alloc memory!");
        return paybuff;
    }

    while (frames) {
        memcpy(&paybuff[offset], PRO_PAYLOAD(frames), PRO_PAYLEN(frames));

        offset += PRO_PAYLEN(frames);
        frames = frames->next;
    }

    *paylen = offset;
    return paybuff;
}

int proFramesParse(ProChan *chan, FrameList **list)
{
    int ret = -EINVAL;
    FrameList *frames = NULL;
    ProHead *phead = NULL;
    char *payload = NULL;
    char *paybuff = NULL;
    int paylen = 0;
    int op1 = -1;
    int op2 = -1;
    int err = -1;

    ASSERT_RET(!chan);
    ASSERT_INF(!chan->read, "Init this protocol channel before destroy!");
    ASSERT_INF(!chan->list_head, "No pending frame list for now!");
    ASSERT_RET(!list);
    ASSERT_RET(!(*list));

    frames  = chan->list_head;
    phead   = PRO_HEAD(frames);
    payload = PRO_PAYLOAD(frames);
    paylen  = PRO_PAYLEN(frames);
    op2     = phead->opcode2;
    op1     = phead->opcode1;
    err     = phead->errcode;

    ret = pro_handle_sub_ack(chan, phead);
    if (ret >= 0)
        goto out;

    if ( is_multi_frame(phead) ) {
        /* multi-frame, merge all payload data into one buffer */
        paybuff = pro_payload_merge(frames, &paylen);
        if (!paybuff) {
            zlog_error(gZlog, "Failed to merge payload buffer, chanid[%d]", chan->chanid);
            ret = -ENOMEM;
            goto out;
        }

        payload = paybuff;
        zlog_debug(gZlog, "Succeed to merge payload buffer, paylen[%d], chanid[%d]",
                paylen, chan->chanid);
    }

    zlog_debug(gZlog, "Got new frame, op1[0x%x], op2[0x%x], err[0x%x], paylen[%d], chanid[%d]",
            op1, op2, err, paylen, chan->chanid);
    
    if (chan->parse)
      ret = chan->parse(chan, payload, paylen, op1, op2, err, frames);

out:
    frame_free(frames);
    chan->list_head = NULL;
    *list = NULL;
    if (paybuff)
        free(paybuff);
    return ret;
}

